//
// Created by geoffrey on 24-11-28.
//

#include "Eular2Isometry.h"

Isometry3d Eular2Isometry(const Matrix<double, 6, 1> x_Eular)
{
    Vector3d eulerAngle = x_Eular.tail(3);

    //欧拉角转旋转向量
    AngleAxisd rollAngle(AngleAxisd(eulerAngle(0),Vector3d::UnitX()));
    AngleAxisd pitchAngle(AngleAxisd(eulerAngle(1),Vector3d::UnitY()));
    AngleAxisd yawAngle(AngleAxisd(eulerAngle(2),Vector3d::UnitZ()));

    //欧拉角转四元数
    Quaterniond quaternion;
    quaternion=yawAngle*pitchAngle*rollAngle;


    Isometry3d x_Isometry (quaternion);
    x_Isometry.pretranslate(Vector3d(x_Eular.head(3)));
    return x_Isometry;
}